from pathlib import Path
from datetime import datetime
from tqdm import tqdm
import shutil


def get_current_time() -> str:
    # 获取当前时间
    current_time = datetime.now()
    # 格式化当前时间为指定格式的字符串
    formatted_time = current_time.strftime("%Y-%m-%d %H:%M:%S")
    return formatted_time


# file_path = "D://FY_沈阳测试//7.5 机场测试数据//MCmanager_7_2//mcm//src//MCManager.INFO"
# file_path = "C://Users//22076//Desktop//7.9测试//7_9第一次MCManager.log.INFO"
def combine_mcm_log(file_path, index):
    '''
    按顺序合并日志文件
    :param file_path: 存放日志的文件路径
    :param index: 日志文件的数量
    :return: None
    '''

    current_time = str(datetime.now())

    print(current_time.split('.')[0].replace(' ', '_').replace(':', '-'))
    target_file_name = current_time.split('.')[0].replace(' ', '_').replace(':', '-')

    # 打开目标文件
    with open(target_file_name, 'wb') as target_file:
        for i in index:
            if i == 0:
                file_name = f"MCManager.log.INFO"
            else:
                file_name = f"MCManager.log.INFO.{i}"

            if not Path(file_path / file_name).exists():
                print(f"{file_name}文件不存在")
                continue

            with open(file_path / file_name, 'rb') as source_file:
                shutil.copyfileobj(source_file, target_file)
    pass


def extarct_mcm_info(file_path: str):
    """
    提取MCM日志中的96pkgs字节信息信息
    :param file_path: 保存日志的路径
    :return:
    """
    extracted_contents = []
    keyword = "96 pkgs info:"

    with open(file_path, 'r', encoding='utf-8') as file:
        for line in file:
            # print(line)
            index = line.find(keyword)
            if index != -1:
                # 提取关键词之后的内容（不包括关键词本身）
                extracted_contents.append(line[index + len(keyword):].strip())

    # 文件保存 保存至主目录的data目录下的 rgb或thermal数据中
    current_path = Path(__file__).resolve()
    parent_path = current_path.parent.parent
    folder = parent_path / "test" / "96pkgs_info"

    index = 0

    for content in tqdm(extracted_contents, desc="Extracting 96pkgs_info: "):
        new_file_name = f"file_{index}.txt"

        if not folder.exists():
            folder.mkdir()
        new_file = folder / new_file_name
        if not new_file.exists():
            new_file.touch()

        with open(new_file, "w") as file:
            file.write(content)
        # print(content)
        index += 1

    print("96pkgs_info提取完成")
    # combine_96pkgs_data_mc_origin(str(folder), len(extracted_contents))
    print(len(extracted_contents))


def extart_mcm_fly_data(file_path: str):
    """
    提取任务机日志中的飞控数据，解析合并为一个TXT文件
    :param file_path: 日志的文件位置
    :return:
    """
    extracted_contents = []
    keyword = "driver_mng_fly_ctrl.cpp:262] "
    type = "fly_data"

    with open(file_path, 'r', encoding='utf-8') as file:
        for line in file:
            # print(line)
            index = line.find(keyword)
            if index != -1:
                # 提取关键词之后的内容（不包括关键词本身）
                extracted_contents.append(line[index + len(keyword):].strip())

    # 文件保存 保存至主目录的data目录下的 rgb或thermal数据中
    current_path = Path(__file__).resolve()
    parent_path = current_path.parent.parent
    folder = parent_path / "test" / type

    index = 0

    for content in tqdm(extracted_contents, desc=f"Extracting {type}: "):
        new_file_name = f"file_{index}.txt"

        if not folder.exists():
            folder.mkdir()
        new_file = folder / new_file_name
        if not new_file.exists():
            new_file.touch()

        with open(new_file, "w") as file:
            file.write(content)
        # print(content)
        index += 1

    print(f"{type}提取完成")
    # combine_96pkgs_data_mc_origin(str(folder), len(extracted_contents))
    file_counts = len(extracted_contents)
    print(file_counts)

    # 将文件夹下的文件合并到z_submit_fly_data_time目录下
    # 并使用时间戳进行命名
    time_str = get_current_time().replace(" ", "-").replace(":", "_")
    # 新建文件
    new_combine_file = parent_path / "data" / "z_submit_fly_data_time" / (time_str + "_fly_data.txt")

    if not new_combine_file.exists():
        new_combine_file.touch()

    with open(new_combine_file, "w", encoding="utf-8") as combine_file:
        combine_file.write(
            "序号,航向角,俯仰角,倾斜角,空速,气压高度,GPS地速,"
            "纬度,经度,GPS高度,航迹角,年,月,日,时,分,秒,"
            "起飞点海拔,无人机东向速度,无人机北向速度,无人机天向速度,"
        )

    for i in tqdm(range(file_counts), desc="Saving MCM fly data files: "):

        temp_data = []
        list_value = []
        list_value.append(str(i))
        fly_data_file_path = str(folder) + "//file_" + str(i) + ".txt"

        with open(fly_data_file_path, "r", encoding="utf-8") as file:
            line = file.readline()
            temp_data = bytes.fromhex(line)

        if len(temp_data) > 0:
            # 飞控数据解析
            if temp_data[0] == 0x9F and temp_data[1] == 0xE4:
                ########################################
                # fly data 64 bytes                    #
                ########################################

                index_low = 1
                index_high = 0

                # 航向角 psi 直接对低八位进行运算
                psi = temp_data[22 - 1:23]
                psi = (psi[index_low] & 0x0F) << 8 | psi[index_high]
                psi = psi / 10.0 + 10.0
                if psi > 360.0:
                    psi = psi - 360.0
                list_value.append(str(psi))

                # 俯仰角 pit
                pit = temp_data[24 - 1:25]
                pit = pit[index_low] << 8 | pit[index_high]
                pit = pit * 156.0 / 4095.0 - 78.0
                list_value.append(str(pit))

                # 倾斜角 rol
                rol = temp_data[26 - 1:27]
                rol = rol[index_low] << 8 | rol[index_high]
                rol = rol * 156.0 / 4095.0 - 78.0
                list_value.append(str(rol))

                # 空速 v 0.01m/s
                v = temp_data[28 - 1:29]
                v = v[index_low] << 8 | v[index_high]
                v = v / 100.0
                list_value.append(str(v))

                # 气压高度
                h = temp_data[30 - 1:31]
                h = h[index_low] << 8 | h[index_high]
                h = h * 2.5 - 300
                list_value.append(str(h))

                # GPS地速 0.01m/s
                v_gps = temp_data[32 - 1:33]
                v_gps = v_gps[index_low] << 8 | v_gps[index_high]
                v_gps = v_gps / 100.0
                list_value.append(str(v_gps))

                # 维度 0.001 首位为标志位 保留9位小数点
                lat = temp_data[34 - 1:37]
                bits_lat_1 = lat[3] & 0x80 != 0
                if bits_lat_1 is True:  # 首位0
                    lat = lat[3] << 24 | lat[2] << 16 | lat[1] << 8 | lat[0]
                    lat = -(lat / 1000.0) / 3600.0
                    lat = format(lat, '.9f')
                else:  # 首位1 改为0在计算 带上+
                    buffer = bytearray(lat)
                    buffer[3] &= 0x7F
                    lat = buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]
                    lat = lat / 1000.0 / 3600.0
                    lat = format(lat, '.9f')
                list_value.append(str(lat))

                lon = temp_data[38 - 1:41]
                bits_lon_1 = lon[3] & 0x80 != 0
                if bits_lon_1 is True:  # 首位0
                    lon = lon[3] << 24 | lon[2] << 16 | lon[1] << 8 | lon[0]
                    lon = -(lon / 1000.0) / 3600.0
                    lon = format(lon, '.9f')
                else:  # 首位1 改为0在计算 带上+
                    buffer = bytearray(lon)
                    buffer[3] &= 0x7F
                    lon = buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]
                    lon = lon / 1000.0 / 3600.0
                    lon = format(lon, '.9f')
                list_value.append(str(lon))

                # GPS高度
                H_gps = temp_data[42 - 1:43]
                H_gps = H_gps[index_low] << 8 | H_gps[index_high]
                H_gps = H_gps * 1 - 1000
                list_value.append(str(H_gps))

                # 航迹角
                Psi_gs = temp_data[44 - 1:45]
                Psi_gs = Psi_gs[index_low] << 8 | Psi_gs[index_high]
                Psi_gs = Psi_gs / 10.0
                list_value.append(str(Psi_gs))

                # 年月日
                year = temp_data[46 - 1]
                month = temp_data[47 - 1]
                day = temp_data[48 - 1]
                list_value.append(str(year))
                list_value.append(str(month))
                list_value.append(str(day))

                # 时分秒 +8改为utc时间
                hour = temp_data[49 - 1] + 8
                minute = temp_data[50 - 1]
                second = temp_data[51 - 1]
                list_value.append(str(hour))
                list_value.append(str(minute))
                list_value.append(str(second))

                # 起飞点海拔
                BY = temp_data[52 - 1:53]
                BY = BY[index_low] << 8 | BY[index_high]
                list_value.append(str(BY))

                # 无人机东向速度
                v_east = temp_data[58 - 1:59]
                v_east = v_east[index_low] << 8 | v_east[index_high]
                v_east = v_east * 200 / 32768
                list_value.append(str(v_east))

                # 无人机北向速度
                v_north = temp_data[60 - 1:61]
                v_north = v_north[index_low] << 8 | v_north[index_high]
                v_north = v_north * 200 / 32768
                list_value.append(str(v_north))

                # 无人机天向速度
                v_up = temp_data[62 - 1:63]
                v_up = v_up[index_low] << 8 | v_up[index_high]
                v_up = v_up * 200 / 32768
                list_value.append(str(v_up))

                fly_data_by = temp_data[64 - 1]

                if len(list_value) > 0:
                    with open(new_combine_file, "a", encoding="utf-8") as combine_file:
                        combine_file.write("\n" + " ".join(list_value))

    print(f"飞控数据文件已经合并到{new_combine_file}中")


if __name__ == '__main__':
    # file_path = "D://FY_沈阳测试//7_14 机场测试数据//96pkgs"
    file_path = "D://AppData//WeChatFiles//WeChat Files//wxid_fggctl0jq3r822//FileStorage//File//2024-07//MCManager.log(2).INFO"
    extart_mcm_fly_data(file_path)
    # extarct_mcm_info(file_path)
